// stl includes
#include <algorithm>
#include <set>
#include <vector>

enum nodestate{open,visited,closed};
struct Node
{
public:
	double x,y; // world coordinates
	int xInt, yInt; // position in neighbor matrix
	nodestate state;
	double cost;
};

class Dijkstra
{
private:

	// Finds any of v's neighbors
	void addNeighboursToStack(std::vector< int > &openList, std::vector<std::vector<bool>> &graph, std::vector<Node> &nodeList, int v)
	{
		for(size_t i=0; i<nodeList.size(); i++){
			if(i==v) continue; // don't add self

			if( graph[v][i] || graph[i][v] )
			{
				if( nodeList[i].state == open )
				{
					nodeList[i].state = visited;

					openList.push_back(i);
					printf("pushet %c\n", i+65 );

					double newCost = nodeList[v].cost + 1;
					nodeList[i].cost = newCost;
					predecessorList[i] = v;
				}

			}
		}
	}



public:
	bool dijkstra(std::vector<std::vector<bool>> &graph, std::vector<Node> &nodeList, int start, int goal)
	{
		// init dijkstra
		for(unsigned i=0; i<nodeList.size(); i++)
		{
			nodeList[i].cost = 1e32; // initially all nodes are infinitely expensive
			nodeList[i].state = open; // all nodes can be visited
		}

		this->nodeListCopy = nodeList; // TODO. bad to copy whole datastructure. C++ was made for pointers... not indices into arrays.
		nodeList[start].cost = 0;

		std::vector<int> openList;
		openList.clear();
		openList.push_back( start );
		predecessorList.resize(nodeList.size());

		while( !openList.empty() )
		{
			std::sort(openList.begin(), openList.end(), (*this) );
			int v = openList.back();
			openList.pop_back();

			printf("fjernet %c\n", v+65 );

			nodeList[v].state = closed; // sett closed
			if( v == goal )
			{
				return true;
			}
			addNeighboursToStack(openList, graph, nodeList, v);
		}
		return false;
	}

	// best nederst
	bool operator() (int i, int j) { 
		return ( nodeListCopy[i].cost > nodeListCopy[j].cost ); 
	}

private:
	std::vector<Node> nodeListCopy;
	
public:
	std::vector<int> predecessorList;
};